import cv2
import time
import numpy as np
import traceback
import datetime

from Plot import PltTool
import PathDect

MAP_WIDTH = 190
MAP_HEIGHT = 190
MAP_DATA = np.asarray([
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0],
    [0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1],
    [0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0],
    [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0],
    [0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0],
    [1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1],
    [0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0],
    [0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0],
    [0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    [0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0],
    [0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0],
    [0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0],
    [1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1],
    [0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0],
    [0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0],
    [0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0],
    [1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0],
    [0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
])

START_POINT = (19, 0)
END_POINT = (1, 20)


class MazeTreasureHunt:
    plot = None
    img = None
    template = None
    debug = False

    def LoadImgResource(self, img):
        self.img = img

    def LoadTemplateResource(self, img):
        self.template = img

    def SetPlotUtilsEnable(self, flag):
        if self.plot is None:
            self.plot = PltTool()
        self.plot.Init(flag)

    def SetDebugEnable(self, flag):
        self.debug = flag

    # 删除模板匹配中多余的数据
    @staticmethod
    def RemoveItems(items):
        indexArr = [0]
        resArr = []
        pointX = []
        pointY = []

        for index in range(0, len(items)):
            pointX.append(items[index][0])
            pointY.append(items[index][1])

        for index in range(0, len(items) - 1):
            if abs(items[index][0] - items[index + 1][0]) > 20 and abs(items[index][1] - items[index + 1][1]) > 20:
                indexArr.append(index)
        indexArr.append(-1)

        for index in range(0, len(indexArr) - 1):
            pointSubX = pointX[indexArr[index]:indexArr[index + 1]]
            pointSubY = pointY[indexArr[index]:indexArr[index + 1]]
            pointSubXCounts = np.bincount(pointSubX)
            pointSubXVaule = np.argmax(pointSubXCounts)
            pointSubYCounts = np.bincount(pointSubY)
            pointSubYVaule = np.argmax(pointSubYCounts)
            resArr.append((pointSubXVaule, pointSubYVaule))
        return resArr

    @staticmethod
    def LocateImagePoints(image):
        # 将图像转换为浮点数类型
        image_float = image.astype(float)
        # 对图像进行亮度调整
        enhanced = image_float + 35
        # 将像素值限制在[0, 255]范围内
        enhanced = np.clip(enhanced, 0, 255)
        # 将图像转换为8位无符号整数类型
        enhanced = enhanced.astype(np.uint8)
        # 将图像转换为灰度图像
        gray = cv2.cvtColor(enhanced, cv2.COLOR_BGR2GRAY)
        # 使用二值化将灰度图像转换为黑白图像
        _, binary = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY)
        # 定义结构元素进行开操作
        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
        opened = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel)
        # 使用连通性分析检测二维码边界框
        _, labels, stats, _ = cv2.connectedComponentsWithStats(opened)
        pointArr = []
        # 遍历连通区域
        for stat in stats[1:]:
            x, y, w, h, area = stat
            # 过滤掉面积较小的连通区域
            if 1000 > area > 200:
                # 提取角点坐标
                points = [(x, y), (x + w, y), (x + w, y + h), (x, y + h), (x + y)]
                pointArr.append(points)
                cv2.rectangle(opened, (x, y), (x + w, y + h), 100, 10)

        if len(pointArr) != 4:
            raise Exception("Match feature points failed.")

        # 按照 左上 右上 左下 右下的顺序排序
        points_out = sorted(pointArr, key=lambda x: x[4])
        if points_out[1][0][0] > points_out[2][0][0]:  # 比较x坐标
            point = points_out[1]
            points_out[1] = points_out[2]
            points_out[2] = point

        return opened, points_out

    # 模板匹配
    @staticmethod
    def MatchTemplate(imgSrc, template):
        w, h = template.shape[::-1]
        res = cv2.matchTemplate(imgSrc, template, cv2.TM_CCOEFF_NORMED)
        threshold = 0.6
        loc = np.where(res >= threshold)
        ptArr = list(zip(*loc[::-1]))
        ptArr = MazeTreasureHunt.RemoveItems(ptArr)
        for pt in ptArr:
            cv2.rectangle(imgSrc, pt, (pt[0] + w, pt[1] + h), 100, 10)
        return ptArr

    # 透视变换
    @staticmethod
    def PerspectiveTransform(imgSrc, ptArr):
        pts1 = np.float32(
            [list(ptArr[0][2]),
             list(ptArr[1][1]),
             list(ptArr[2][3]),
             list(ptArr[3][0])]
        )
        pts2 = np.float32([[0, 0], [0, MAP_WIDTH], [MAP_HEIGHT, 0], [MAP_WIDTH, MAP_HEIGHT]])
        M = cv2.getPerspectiveTransform(pts1, pts2)
        warped = cv2.warpPerspective(imgSrc, M, (MAP_WIDTH, MAP_HEIGHT))
        return warped

    @staticmethod
    def CrossPoint(line1, line2):  # 计算交点函数
        x1 = line1[0]  # 取直线1的第一个点坐标
        y1 = line1[1]
        x2 = line1[2]  # 取直线1的第二个点坐标
        y2 = line1[3]

        x3 = line2[0]  # 取直线2的第一个点坐标
        y3 = line2[1]
        x4 = line2[2]  # 取直线2的第二个点坐标
        y4 = line2[3]

        if x2 - x1 == 0:  # L1 直线斜率不存在
            k1 = None
            b1 = 0
        else:
            k1 = (y2 - y1) * 1.0 / (x2 - x1)  # 计算k1,由于点均为整数，需要进行浮点数转化
            b1 = y1 * 1.0 - x1 * k1 * 1.0  # 整型转浮点型是关键

        if (x4 - x3) == 0:  # L2直线斜率不存在操作
            k2 = None
            b2 = 0
        else:
            k2 = (y4 - y3) * 1.0 / (x4 - x3)  # 斜率存在操作
            b2 = y3 * 1.0 - x3 * k2 * 1.0

        if k1 is None and k2 is None:  # L1与L2直线斜率都不存在，两条直线均与y轴平行
            if x1 == x3:  # 两条直线实际为同一直线
                return [x1, y1]  # 均为交点，返回任意一个点
            else:
                return None  # 平行线无交点
        elif k1 is not None and k2 is None:  # 若L2与y轴平行，L1为一般直线，交点横坐标为L2的x坐标
            x = x3
            y = k1 * x * 1.0 + b1 * 1.0
        elif k1 is None and k2 is not None:  # 若L1与y轴平行，L2为一般直线，交点横坐标为L1的x坐标
            x = x1
            y = k2 * x * 1.0 + b2 * 1.0
        else:  # 两条一般直线
            if k1 == k2:  # 两直线斜率相同
                if b1 == b2:  # 截距相同，说明两直线为同一直线，返回任一点
                    return [x1, y1]
                else:  # 截距不同，两直线平行，无交点
                    return None
            else:  # 两直线不平行，必然存在交点
                x = (b2 - b1) * 1.0 / (k1 - k2)
                y = k1 * x * 1.0 + b1 * 1.0
        return [x, y]

    @staticmethod
    def LinesFilter(lines):
        lines_out = []
        lines_filter = []
        for line in lines:
            rho, theta = line[0]  # 获取极值ρ长度和θ角度
            a = np.cos(theta)  # 获取角度cos值
            b = np.sin(theta)  # 获取角度sin值
            x0 = a * rho  # 获取x轴值
            y0 = b * rho  # 获取y轴值　　x0和y0是直线的中点
            x1 = int(x0 + MAP_WIDTH * (-b))  # 获取这条直线最大值点x1
            y1 = int(y0 + MAP_WIDTH * a)  # 获取这条直线最大值点y1
            x2 = int(x0 - MAP_WIDTH * (-b))  # 获取这条直线最小值点x2　　
            y2 = int(y0 - MAP_WIDTH * a)  # 获取这条直线最小值点y2　　其中*1000是内部规则
            line_dected = False
            for line_save in lines_filter:
                if abs(line_save['x1'] - x1) < 10 and abs(line_save['y1'] - y1) < 10:
                    line_dected = True
                    break
            if not line_dected:
                line_save = {'x1': x1, 'y1': y1, 'x2': x2, 'y2': y2}
                lines_filter.append(line_save)
                lines_out.append(line)
        return lines_out

    @staticmethod
    def HoughLines(imgSrc):
        # ret, binary = cv2.threshold(warped, 200, 255, cv2.THRESH_BINARY)  # 二值化
        warped_gray = cv2.cvtColor(imgSrc, cv2.COLOR_BGR2GRAY)
        edges = cv2.Canny(warped_gray, 50, 150, apertureSize=3)  # apertureSize是sobel算子大小，只能为1,3,5，7
        lines = cv2.HoughLines(edges, 1, np.pi / 180, 100)  # 函数将通过步长为1的半径和步长为π/180的角来搜索所有可能的直线
        lines = MazeTreasureHunt.LinesFilter(lines)
        lines_out = []
        for line in lines:
            rho, theta = line[0]  # 获取极值ρ长度和θ角度
            a = np.cos(theta)  # 获取角度cos值
            b = np.sin(theta)  # 获取角度sin值
            x0 = a * rho  # 获取x轴值
            y0 = b * rho  # 获取y轴值　　x0和y0是直线的中点
            x1 = int(x0 + MAP_WIDTH * (-b))  # 获取这条直线最大值点x1
            y1 = int(y0 + MAP_WIDTH * a)  # 获取这条直线最大值点y1
            x2 = int(x0 - MAP_WIDTH * (-b))  # 获取这条直线最小值点x2　　
            y2 = int(y0 - MAP_WIDTH * a)  # 获取这条直线最小值点y2　　其中*1000是内部规则
            line_save = {'x1': x1, 'y1': y1, 'x2': x2, 'y2': y2, 'cos': a}
            cv2.line(warped_gray, (x1, y1), (x2, y2), (0, 0, 255), 2)
            lines_out.append(line_save)
        return lines_out

    @staticmethod
    def GetMapPoints(lines):
        points = []

        # 确保相邻的两条边不平行
        lines_diff = [abs(abs(lines[0]['cos']) - abs(lines[1]['cos'])),
                      abs(abs(lines[0]['cos']) - abs(lines[2]['cos'])),
                      abs(abs(lines[0]['cos']) - abs(lines[3]['cos']))]
        min_index = lines_diff.index(min(lines_diff))
        if min_index != 2:
            lines_pop = lines.pop(min_index)
            lines.insert(2, lines_pop)

        for index in range(0, len(lines) - 1):
            point = MazeTreasureHunt.CrossPoint(
                (lines[index]['x1'], lines[index]['y1'], lines[index]['x2'], lines[index]['y2']), (
                    lines[index + 1]['x1'], lines[index + 1]['y1'], lines[index + 1]['x2'],
                    lines[index + 1]['y2']))
            points.append(point)
        points.append(MazeTreasureHunt.CrossPoint((lines[0]['x1'], lines[0]['y1'], lines[0]['x2'], lines[0]['y2']),
                                                  (lines[-1]['x1'], lines[-1]['y1'], lines[-1]['x2'], lines[-1]['y2'])))

        # 单次插入排序
        for point in points:
            point.append(point[0] + point[1])

        # 按照 左上 右上 左下 右下的顺序排序
        points_out = sorted(points, key=lambda x: x[2])
        if points_out[1][0] > points_out[2][0]:  # 比较x坐标
            point = points_out[1]
            points_out[1] = points_out[2]
            points_out[2] = point

        return np.array(points_out)[:, :2]

    @staticmethod
    def GetTarget(imgSrc):
        # 获取宝藏地点
        targets = []
        k = np.ones((6, 6), np.uint8)  # 定义核
        binary_target = cv2.dilate(imgSrc, k)  # 膨胀
        k = np.ones((9, 9), np.uint8)  # 定义核
        binary_target = cv2.erode(binary_target, k)  # 腐蚀
        imgCanny = cv2.Canny(binary_target, 60, 60)  # Canny算子边缘检测
        contours, hierarchy = cv2.findContours(imgCanny, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)  # 寻找轮廓点
        for obj in contours:
            area = cv2.contourArea(obj)  # 计算轮廓内区域的面积
            perimeter = cv2.arcLength(obj, True)  # 计算轮廓周长
            approx = cv2.approxPolyDP(obj, 0.02 * perimeter, True)  # 获取轮廓角点坐标
            CornerNum = len(approx)  # 轮廓角点的数量
            x, y, w, h = cv2.boundingRect(approx)  # 获取坐标值和宽度、高度
            if 10 < w < 20 and 10 < h < 20:  # 过滤因噪声误识别的点
                # x_pos = np.mean(obj[:, 0, 0])
                # y_pos = np.mean(obj[:, 0, 1])
                x_pos = x + w / 3
                y_pos = y + h / 3
                target = {'x': x_pos, 'y': y_pos, 'points': obj}
                targets.append(target)
        return targets

    @staticmethod
    def SearchAround(map, target):
        # 初始化迷宫的四种走法
        around_step = [[0, 1],  # 向右走
                       [1, 0],  # 向下走
                       [0, -1],  # 向左走
                       [-1, 0],  # 向上走
                       [1, 1],
                       [1, -1],
                       [-1, -1],
                       [-1, 1]
                       ]
        next_step = [[0, 1],  # 向右走
                     [1, 0],  # 向下走
                     [0, -1],  # 向左走
                     [-1, 0]  # 向上走
                     ]
        cnt = 0
        # 当前target的四个方向，有三个为墙壁时，直接判定当前target为对的目标点：
        for dx, dy in next_step:
            cnt = cnt + map[target[0] + dx, target[1] + dy]
        if cnt == 3:
            return target

        for dx, dy in around_step:
            around_point = (target[0] + dx, target[1] + dy)
            if around_point[0] > 19 or around_point[1] > 19:  # 超出边界
                continue
            if map[around_point] == 1:  # 墙壁
                continue

            # 开始判断疑似目标点周围的墙壁数量
            wall_cnt = 0
            for x, y in next_step:
                second_around_point = (around_point[0] + x, around_point[1] + y)
                if second_around_point[0] > 20 or second_around_point[1] > 20:  # 超出边界
                    break
                if map[second_around_point] == 1:  # 墙壁
                    wall_cnt = wall_cnt + 1
                if wall_cnt == 3:
                    return around_point
        return target

    @staticmethod
    def GetTargetsIndex(map, targets):
        target_out = []
        for target in targets:
            target['x_pos'] = int(target['x'] / (MAP_WIDTH / 19) + 1)
            target['y_pos'] = int(target['y'] / (MAP_HEIGHT / 19) + 1)
            real_target = MazeTreasureHunt.SearchAround(map, (int(target['y_pos']), int(target['x_pos'])))
            map[real_target] = 2
            target_out.append(real_target)
        return target_out

    @staticmethod
    def DumpImage(image, path=None):
        filename = None
        if path:
            filename = path
        else:
            # 生成文件名，格式为"日期+时间.jpg"
            current_time = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S.%f")[:-3]
            filename = f"{current_time}.png"
        # 保存帧为图像文件
        cv2.imwrite(filename, image)

    @staticmethod
    def DumpMap(map_data, path=None):
        # 定义颜色映射
        color_map = {
            0: [0, 0, 0],  # 黑色
            1: [255, 255, 255],  # 白色
            2: [0, 0, 255],  # 红色
            3: [0, 255, 0],  # 绿色
        }

        # 创建空白图像
        height, width = map_data.shape
        output_image = np.zeros((height, width, 3), dtype=np.uint8)

        # 根据映射填充颜色
        for i in range(height):
            for j in range(width):
                value = map_data[i, j]
                output_image[i, j] = color_map.get(value, [255, 255, 255])  # 默认为白色

        # 将图像放大20倍
        resized_image = cv2.resize(output_image, (width * 20, height * 20), interpolation=cv2.INTER_NEAREST)
        MazeTreasureHunt.DumpImage(resized_image, path)

    def Process(self):
        if self.img is None:
            raise Exception("Image is empty.")

        if self.plot is None:
            self.plot = PltTool()
        # 转换为灰度图
        imgGary = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY)
        self.plot.ImagePltConf(3, 5, 1, self.img)

        # 对迷宫的bbox进行模板匹配
        openedImg, ptArr = self.LocateImagePoints(self.img)
        if self.debug:
            cv2.imshow("locate points", openedImg)
        if len(ptArr) != 4:
            raise Exception("Match feature points failed.")

        # 对迷宫bbox进行透视变换
        warped = self.PerspectiveTransform(self.img, ptArr)
        self.plot.ImagePltConf(3, 5, 2, warped)
        if self.debug:
            cv2.imshow("Pers Trans 1", warped)

        # 对迷宫边框进行透视变化
        lines_out = self.HoughLines(warped)
        if len(lines_out) != 4:
            raise Exception("Match frame failed.")
        points = self.GetMapPoints(lines_out)
        pts1 = np.float32(points)
        pts2 = np.float32([[0, 0], [0, MAP_HEIGHT], [MAP_WIDTH, 0], [MAP_WIDTH, MAP_HEIGHT]])
        M = cv2.getPerspectiveTransform(pts1, pts2)
        warped = cv2.warpPerspective(warped, M, (MAP_WIDTH, MAP_HEIGHT))
        self.plot.ImagePltConf(3, 5, 3, warped)
        if self.debug:
            cv2.imshow("Pers Trans 2", warped)

        # 处理藏宝图墙壁部分
        warped_gray = cv2.cvtColor(warped, cv2.COLOR_BGR2GRAY)
        cv2.medianBlur(warped_gray, 5)
        ret, binary = cv2.threshold(warped, 100, 255, cv2.THRESH_BINARY)  # 二值化
        k = np.ones((8, 8), np.uint8)  # 定义核
        binary_path = cv2.erode(binary, k)  # 腐蚀

        targets = self.GetTarget(binary)
        binary_complate = binary_path.copy()
        for target in targets:
            cv2.drawContours(binary_complate, target['points'], -1, (255, 0, 0), 4)  # 绘制轮廓线
        self.plot.ImagePltConf(3, 5, 4, binary_complate)

        map_data = PathDect.ExpandMapData(MAP_DATA)
        targets_arr = self.GetTargetsIndex(map_data, targets)
        self.plot.ImagePltConf(3, 5, 5, map_data)
        print(targets_arr)
        for target in targets_arr:
            if map_data[target] == 1:
                raise Exception("Failed to locate target point.")
        targets_arr = PathDect.ParseShortestPath(map_data, targets_arr, START_POINT, END_POINT)

        all_paths = {'CommonPath': [], 'StartPath': [], 'EndPath': []}

        all_length = 0
        for index in range(0, len(targets_arr) - 1):
            length, path = PathDect.BFSearch(map_data, targets_arr[index], targets_arr[index + 1])
            out_path = PathDect.GetBFSPath(path, targets_arr[index + 1])
            out_path.reverse()
            all_length = all_length + length
            print("allLen:", all_length, "len:", length, "path:", out_path)
            all_paths['CommonPath'].append(out_path)
            result_path = map_data.copy()
            for point in out_path:
                result_path[point] = 3
            self.plot.ImagePltConf(3, 5, index + 6, result_path)

        for target in targets_arr[1: -1]:
            length, path = PathDect.BFSearch(map_data, START_POINT, target)
            out_path = PathDect.GetBFSPath(path, target)
            out_path.reverse()
            all_paths['StartPath'].append(out_path)
            result_path = map_data.copy()
            for point in out_path:
                result_path[point] = 3

        for target in targets_arr[1: -1]:
            length, path = PathDect.BFSearch(map_data, target, END_POINT)
            out_path = PathDect.GetBFSPath(path, END_POINT)
            out_path.reverse()
            all_paths['EndPath'].append(out_path)
            result_path = map_data.copy()
            for point in out_path:
                result_path[point] = 3

        self.plot.PltShow()
        return all_paths

    @staticmethod
    def GetTrea():
        # 定义红色、绿色、蓝色和黄色的HSV范围
        lower_red = np.array([0, 100, 100])
        upper_red = np.array([10, 255, 255])
        lower_green = np.array([45, 40, 40])
        upper_green = np.array([80, 220, 220])
        lower_blue = np.array([60, 80, 140])
        upper_blue = np.array([120, 140, 200])
        lower_yellow = np.array([16, 120, 120])
        upper_yellow = np.array([43, 255, 255])
        xr = xy = xb = xg = 0
        yr = yy = yb = yg = 0
        wr = wy = wb = wg = 0
        hr = hy = hb = hg = 0
        resultT = [0x41]
        # 初始化摄像头
        cap = cv2.VideoCapture(0, cv2.CAP_V4L)
        cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        cnt = 0
        while True:
            # 从摄像头中获取图像
            ret, frame = cap.read()
            # key = cv2.waitKey(1) & 0xFF
            cnt += 1
            if ret:
                # 转换图像为HSV格式
                hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
                print(cnt)
                # 红色检测
                red_mask = cv2.inRange(hsv, lower_red, upper_red)
                red_contours, _ = cv2.findContours(red_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
                red_contours = sorted(red_contours, key=lambda x: cv2.contourArea(x), reverse=True)
                if len(red_contours) > 0:
                    (xr, yr, wr, hr) = cv2.boundingRect(red_contours[0])
                    cv2.rectangle(frame, (xr, yr), (xr + wr, yr + hr), (0, 0, 255), 2)
                    cv2.putText(frame, "Red", (xr, yr), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

                # 绿色检测
                green_mask = cv2.inRange(hsv, lower_green, upper_green)
                green_contours, _ = cv2.findContours(green_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
                green_contours = sorted(green_contours, key=lambda x: cv2.contourArea(x), reverse=True)
                if len(green_contours) > 0:
                    (xg, yg, wg, hg) = cv2.boundingRect(green_contours[0])
                    cv2.rectangle(frame, (xg, yg), (xg + wg, yg + hg), (0, 255, 0), 2)
                    cv2.putText(frame, "Green", (xg, yg), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)

                # 蓝色检测
                blue_mask = cv2.inRange(hsv, lower_blue, upper_blue)
                blue_contours, _ = cv2.findContours(blue_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
                blue_contours = sorted(blue_contours, key=lambda x: cv2.contourArea(x), reverse=True)
                if len(blue_contours) > 0:
                    (xb, yb, wb, hb) = cv2.boundingRect(blue_contours[0])
                    cv2.rectangle(frame, (xb, yb), (xb + wb, yb + hb), (255, 0, 0), 2)
                    cv2.putText(frame, "Blue", (xb, yb), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)

                # 黄色检测
                yellow_mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
                yellow_contours, _ = cv2.findContours(yellow_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
                yellow_contours = sorted(yellow_contours, key=lambda x: cv2.contourArea(x), reverse=True)
                if len(yellow_contours) > 0:
                    (xy, yy, wy, hy) = cv2.boundingRect(yellow_contours[0])
                    cv2.rectangle(frame, (xy, yy), (xy + wy, yy + hy), (0, 255, 255), 2)
                    cv2.putText(frame, "Yellow", (xy, yy), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)

            if xr < xg and xr + wr > xg + wg and yr < yg and yr + hr > yg + hg:
                resultT.insert(0, 0x52)
                break
            elif xb < xy and xb + wb > xy + wy and yb < yy and yb + hb > yy + hy:
                resultT.insert(0, 0x42)
                break
            elif xr < xy and xr + wr > xy + wy and yr < yy and yr + hr > yy + hy:
                resultT.insert(0, 0x46)
                break
            elif xb < xg and xb + wb > xg + wg and yb < yg and yb + hb > yg + hg:
                resultT.insert(0, 0x46)
                break
            elif cnt == 100:
                resultT.insert(0, 0x46)
                break
            # 显示识别结果
            # cv2.imshow("Color Detection", frame)
            xr = xy = xb = xg = 0
            yr = yy = yb = yg = 0
            wr = wy = wb = wg = 0
            hr = hy = hb = hg = 0
        # 释放摄像头
        cap.release()

        # 关闭所有窗口
        cv2.destroyAllWindows()
        return resultT

    @staticmethod
    def clean_lists(lists):
        cleaned_lists = [[x for i, x in enumerate(lst) if i >= 2 and (i % 2 == 0 or x != 3)] for lst in lists]
        return cleaned_lists


if __name__ == "__main__":
    mazeHunt = MazeTreasureHunt()
    mazeHunt.SetPlotUtilsEnable(True)                                          # 关闭绘图

    try:
        start_time = time.time()
        mazeHunt.LoadImgResource(cv2.imread("./image/img_1.png"))
        lenSum, paths = mazeHunt.Process()
    except Exception as e:
        print("Get Error: " + str(e))
        traceback.print_exc()
    else:
        end_time = time.time()
        print("Process Run Time: ", end_time - start_time)
    cv2.waitKey(0)
